#include "State_JumpL.h"
#include "Dog.h"

#define ANGLE_F RAD(70) // 缩腿站立
#define ANGLE_B RAD(80)
#define preANGLE2_F RAD(40) // 在缩腿站立状态下需继续旋转的角度
#define preANGLE2_B RAD(40)

#define KP1 8.0f
#define KD1 1.3f
#define KP2 8.0f
#define KD2 1.3f

void JumpL_Enter(void)
{
    UART_Print("JumpL enter\n");
}

void JumpL_Run(void)
{
    /*曲腿准备*/
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F - preANGLE2_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F - preANGLE2_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F - preANGLE2_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F - preANGLE2_F, KP1, KD1, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B + preANGLE2_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B + preANGLE2_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B + preANGLE2_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B + preANGLE2_B, KP1, KD1, 0);
}

void JumpL_Exit(void)
{
    UART_Print("JumpL exit\n");
}